/******************************************************/
/* Goblin's Cube - Rubik's cube game for Wii Homebrew */
/* Copyright (C) 2009 Ricardo Bueno Cordeiro          */
/* Licensed under de GNU General Public License v2.0  */
/*   See license_gplv2.txt for detalis                */
/******************************************************/

#include <gccore.h>
#include <ogc/tpl.h>
#include <ogc/lwp_watchdog.h>

#include "cube.h"
#include "draw.h"
#include "random.h"

#include "sound.h"
#include "sound_ogg_loader.h"

#include "cube1_ogg.h"
#include "cube2_ogg.h"
#include "cube3_ogg.h"
#include "cube4_ogg.h"

Cube Cube::cube;

// Axis
static Vector X = {1,0,0};
static Vector Y = {0,1,0};
static Vector Z = {0,0,1};

// Texture coordinates
static f32 texture_coord[][2] = {
	{     0.0,     0.0 },     // white
	{ 1.0/3.0,     0.0 },   // yellow
	{ 2.0/3.0,     0.0 },   // red
	{     0.0, 1.0/3.0 },   // blue
	{ 1.0/3.0, 1.0/3.0 }, // orange
	{ 2.0/3.0, 1.0/3.0 }  // green
};

// Instruction inital values
static Instruction instruction_templates[] = {
	{ INSTRUCTION_TURN_LEFT,  0, 0, NUM_ANIMATION_STEPS, &Y,   -90.0, 0 },
	{ INSTRUCTION_TURN_RIGHT, 0, 0, NUM_ANIMATION_STEPS, &Y,    90.0, 0 },
	{ INSTRUCTION_TURN_UP,    0, 0, NUM_ANIMATION_STEPS, &X,   -90.0, 0 },
	{ INSTRUCTION_TURN_DOWN,  0, 0, NUM_ANIMATION_STEPS, &X,    90.0, 0 },
	{ INSTRUCTION_TURN_CW,    0, 0, NUM_ANIMATION_STEPS, &Z,   -90.0, 0 },
	{ INSTRUCTION_TURN_CC,    0, 0, NUM_ANIMATION_STEPS, &Z,    90.0, 0 },
	{ INSTRUCTION_ROTATE_CW,  0, 0, NUM_ANIMATION_STEPS, NULL,   0.0, 0 },
	{ INSTRUCTION_ROTATE_CC,  0, 0, NUM_ANIMATION_STEPS, NULL,   0.0, 0 },
	{ INSTRUCTION_UNDO,       0, 0, NUM_ANIMATION_STEPS, NULL,   0.0, 0 }
};

// Instruction members by type
struct Cube::instruction_members_t Cube::instruction_members[] = {
	{ &Cube::init_turn,            &Cube::apply_turn_left,       &Cube::apply_turn_right },
	{ &Cube::init_turn,            &Cube::apply_turn_right,      &Cube::apply_turn_left },
	{ &Cube::init_turn,            &Cube::apply_turn_up,         &Cube::apply_turn_down },
	{ &Cube::init_turn,            &Cube::apply_turn_down,       &Cube::apply_turn_up },
	{ &Cube::init_turn,            &Cube::apply_turn_cw,         &Cube::apply_turn_cc },
	{ &Cube::init_turn,            &Cube::apply_turn_cc,         &Cube::apply_turn_cw },
	{ &Cube::init_rotate_piece_cw, &Cube::apply_rotate_piece_cw, &Cube::apply_rotate_piece_cc },
	{ &Cube::init_rotate_piece_cc, &Cube::apply_rotate_piece_cc, &Cube::apply_rotate_piece_cw },
	{ &Cube::init_undo,            &Cube::apply_undo,            NULL },
	{ NULL, NULL, NULL }
};	

Cube::Cube() {
	// Set cube structure (this never changes)
	this->face[0].edge[0] = 0;
	this->face[0].edge[1] = 3;
	this->face[0].edge[2] = 2;
	this->face[0].edge[3] = 1;

	this->face[1].edge[0] = 8;
	this->face[1].edge[1] = 11;
	this->face[1].edge[2] = 10;
	this->face[1].edge[3] = 9;

	this->face[2].edge[0] = 0;
	this->face[2].edge[1] = 4;
	this->face[2].edge[2] = 8;
	this->face[2].edge[3] = 7;

	this->face[3].edge[0] = 1;
	this->face[3].edge[1] = 5;
	this->face[3].edge[2] = 11;
	this->face[3].edge[3] = 4;

	this->face[4].edge[0] = 2;
	this->face[4].edge[1] = 6;
	this->face[4].edge[2] = 10;
	this->face[4].edge[3] = 5;

	this->face[5].edge[0] = 3;
	this->face[5].edge[1] = 7;
	this->face[5].edge[2] = 9;
	this->face[5].edge[3] = 6;

	this->edge[0].face[0] = 0;
	this->edge[0].face[1] = 2;

	this->edge[1].face[0] = 0;
	this->edge[1].face[1] = 3;

	this->edge[2].face[0] = 0;
	this->edge[2].face[1] = 4;

	this->edge[3].face[0] = 0;
	this->edge[3].face[1] = 5;

	this->edge[4].face[0] = 2;
	this->edge[4].face[1] = 3;

	this->edge[5].face[0] = 3;
	this->edge[5].face[1] = 4;

	this->edge[6].face[0] = 4;
	this->edge[6].face[1] = 5;

	this->edge[7].face[0] = 5;
	this->edge[7].face[1] = 2;

	this->edge[8].face[0] = 1;
	this->edge[8].face[1] = 2;

	this->edge[9].face[0] = 1;
	this->edge[9].face[1] = 5;

	this->edge[10].face[0] = 1;
	this->edge[10].face[1] = 4;

	this->edge[11].face[0] = 1;
	this->edge[11].face[1] = 3;

	for (int i = 0; i < 6; i++) {
		for (int j = 0; j < 8; j++) {
			this->face[i].owning_piece[j][0] = i;
			for (int k = 1; k < 4; k++) {
				this->face[i].owning_piece[j][k] = -1;
			}
		}

		for (int j = 0; j < 4; j++) {
			int edge = this->face[i].edge[j];
			int adjacent_face = this->get_adjacent_face(i, edge);
			for (int k = 0; k < 3; k++)
				this->add_owning_piece(i, (j * 2 + k) % 8, adjacent_face);
		}
	}

	// Load sounds
	this->cube_sound[0].load(new SoundOggLoader(cube1_ogg, cube1_ogg_size));
	this->cube_sound[1].load(new SoundOggLoader(cube2_ogg, cube2_ogg_size));
	this->cube_sound[2].load(new SoundOggLoader(cube3_ogg, cube3_ogg_size));
	this->cube_sound[3].load(new SoundOggLoader(cube4_ogg, cube4_ogg_size));
	
	// Set to initial positions
	this->scramble();
}

int Cube::get_edge_index(int face, int edge) const {
	for(int i = 0; i < 3; ++i) {
		if (this->face[face].edge[i] == edge)
			return i;
	}
	return 3;
}

int	Cube::get_adjacent_face(int cur_face, int edge) const {
	if (this->edge[edge].face[0] == cur_face)
		return this->edge[edge].face[1];
	return this->edge[edge].face[0];
}

void Cube::add_owning_piece(int face, int color_pos, int piece) {
	for (int i = 0; i < 4; i++) {
		if (this->face[face].owning_piece[color_pos][i] == -1) {
			this->face[face].owning_piece[color_pos][i] = piece;
			return;
		}
	}
}

int Cube::owned_by(int face, int color_pos, int piece) const {
	if ((color_pos < 0) || (color_pos >= 8))
		return face == piece;

	for (int i = 0; i < 4; i++) {
		if (this->face[face].owning_piece[color_pos][i] == piece)
			return 1;
	}

	return 0;
}

void Cube::draw_face_piece(double x_offset, double y_offset, int color) const {
	GX_Position3f32(-0.5f + x_offset, -0.5f + y_offset, 1.5f);
	GX_Normal3f32(0.0, 0.0, 1.0);
	GX_Color4u8(0, 0, 0, 255);
	GX_TexCoord2f32(texture_coord[color][0], texture_coord[color][1]);

	GX_Position3f32( 0.5f + x_offset, -0.5f + y_offset, 1.5f);
	GX_Normal3f32(0.0, 0.0, 1.0);
	GX_Color4u8(0, 0, 0, 255);
	GX_TexCoord2f32(texture_coord[color][0] + 1.0/3.0, texture_coord[color][1]);

	GX_Position3f32( 0.5f + x_offset,  0.5f + y_offset, 1.5f);
	GX_Normal3f32(0.0, 0.0, 1.0);
	GX_Color4u8(0, 0, 0, 255);
	GX_TexCoord2f32(texture_coord[color][0] + 1.0/3.0, texture_coord[color][1] + 1.0/3.0);

	GX_Position3f32(-0.5f + x_offset,  0.5f + y_offset, 1.5f);
	GX_Normal3f32(0.0, 0.0, 1.0);
	GX_Color4u8(0, 0, 0, 255);
	GX_TexCoord2f32(texture_coord[color][0], texture_coord[color][1] + 1.0/3.0);
}

int Cube::must_draw_face_piece(int face, int color_pos, int type, int piece) const {
	if (type == CUBE_DRAW_ALL_PIECES)
		return 1;

	if (type == CUBE_DRAW_THIS_PIECE)
		return this->owned_by(face, color_pos, piece);

	return !this->owned_by(face, color_pos, piece);
}

void Cube::draw_face(int face, f32 rotX, f32 rotY, f32 rotZ, int type, int piece) const {
	Mtx current, model, temp;
	DR_getCurrentModelMtx(current);

	guMtxRotAxisDeg(model, &X, rotX);
	guMtxRotAxisDeg(temp, &Y, rotY);
	guMtxConcat(model,temp,model);
	guMtxRotAxisDeg(temp, &Z, rotZ);
	guMtxConcat(model,temp,model);
	guMtxConcat(current,model,model);
	DR_setCurrentModelMtx(model);

	int num_quad = 0;
	if (this->must_draw_face_piece(face, -1, type, piece))
		num_quad++;
	for (int i = 0; i < 8; i++)
		if (this->must_draw_face_piece(face, i, type, piece))
			num_quad++;

	GX_Begin(GX_QUADS, GX_VTXFMT0, num_quad * 4);

	if (this->must_draw_face_piece(face, -1, type, piece))
		this->draw_face_piece( 0.0, 0.0, this->face[face].central_color);

	if (this->must_draw_face_piece(face, 0, type, piece))
		this->draw_face_piece(-1.0, 1.0, this->face[face].color[0]);
	if (this->must_draw_face_piece(face, 1, type, piece))
		this->draw_face_piece( 0.0, 1.0, this->face[face].color[1]);
	if (this->must_draw_face_piece(face, 2, type, piece))
		this->draw_face_piece( 1.0, 1.0, this->face[face].color[2]);
	if (this->must_draw_face_piece(face, 3, type, piece))
		this->draw_face_piece( 1.0, 0.0, this->face[face].color[3]);
	if (this->must_draw_face_piece(face, 4, type, piece))
		this->draw_face_piece( 1.0,-1.0, this->face[face].color[4]);
	if (this->must_draw_face_piece(face, 5, type, piece))
		this->draw_face_piece( 0.0,-1.0, this->face[face].color[5]);
	if (this->must_draw_face_piece(face, 6, type, piece))
		this->draw_face_piece(-1.0,-1.0, this->face[face].color[6]);
	if (this->must_draw_face_piece(face, 7, type, piece))
		this->draw_face_piece(-1.0, 0.0, this->face[face].color[7]);

	GX_End();

	DR_setCurrentModelMtx(current);
}

void Cube::draw_cube(int type, int piece) const {
	int top_edge_index, edge_index, adjacent_face, adjacent_edge;

	// Draw front
	top_edge_index = this->get_edge_index(this->front_face, this->top_edge);
	this->draw_face(this->front_face, 0.0f, 0.0f, CB_Z_ROT_FOR_TOP_EDGE(top_edge_index), type, piece);

	// Draw top
	adjacent_edge = this->face[this->front_face].edge[top_edge_index];
	adjacent_face = this->get_adjacent_face(this->front_face, adjacent_edge);
	edge_index = this->get_edge_index(adjacent_face, adjacent_edge);
	this->draw_face(adjacent_face, -90.0f, 0.0f, CB_Z_ROT_FOR_TOP_EDGE(edge_index + 2), type, piece);

	// Draw right
	adjacent_edge = this->face[this->front_face].edge[(top_edge_index + 1) % 4];
	adjacent_face = this->get_adjacent_face(this->front_face, adjacent_edge);
	edge_index = this->get_edge_index(adjacent_face, adjacent_edge);
	this->draw_face(adjacent_face, 0.0f, 90.0f, CB_Z_ROT_FOR_TOP_EDGE(edge_index + 1), type, piece);

	// Draw bottom
	adjacent_edge = this->face[this->front_face].edge[(top_edge_index + 2) % 4];
	adjacent_face = this->get_adjacent_face(this->front_face, adjacent_edge);
	edge_index = this->get_edge_index(adjacent_face, adjacent_edge);
	this->draw_face(adjacent_face, 90.0f, 0.0f, CB_Z_ROT_FOR_TOP_EDGE(edge_index), type, piece);

	// Draw left
	adjacent_edge = this->face[this->front_face].edge[(top_edge_index + 3) % 4];
	adjacent_face = this->get_adjacent_face(this->front_face, adjacent_edge);
	edge_index = this->get_edge_index(adjacent_face, adjacent_edge);
	this->draw_face(adjacent_face, 0.0f, -90.0f, CB_Z_ROT_FOR_TOP_EDGE(edge_index + 3), type, piece);

	// Draw back
	adjacent_edge = this->face[this->front_face].edge[top_edge_index];
	adjacent_face = this->get_adjacent_face(this->front_face, adjacent_edge);
	edge_index = this->get_edge_index(adjacent_face, adjacent_edge);
	adjacent_edge = this->face[adjacent_face].edge[(edge_index + 2) % 4];
	adjacent_face = this->get_adjacent_face(adjacent_face, adjacent_edge);
	edge_index = this->get_edge_index(adjacent_face, adjacent_edge);
	this->draw_face(adjacent_face, 0.0f, 180.0f, CB_Z_ROT_FOR_TOP_EDGE(edge_index), type, piece);
}

void Cube::draw() {
	DR_set_cube_texture();
	GX_SetCullMode(GX_CULL_FRONT);

	// No animation needed, just draw and leave
	if (this->current_inst >= this->num_inst) {
		this->draw_cube(CUBE_DRAW_ALL_PIECES, 0);
		return;
	}

	int current = this->current_inst;
	int type = this->instruction[current].type;

	// Initiate instruction if needed
	if (!this->instruction[current].initiated) {
		(this->*(instruction_members[type].init))(current);
		this->instruction[current].initiated = 1;
	}

	Mtx current_mtx, model;

	DR_getCurrentModelMtx(current_mtx);

	int step = this->instruction[current].step;
	f32 revert = 1.0;

	if (type == INSTRUCTION_UNDO) {
		current--;
		revert = -1.0;
	}

	if (this->instruction[current].type <= INSTRUCTION_TURN_CC) {
		guMtxRotAxisDeg(model,
				this->instruction[current].axis,
				revert * this->instruction[current].angle * ((f32)(NUM_ANIMATION_STEPS - step)) / ((f32)NUM_ANIMATION_STEPS));
		guMtxConcat(current_mtx, model, model);
		DR_setCurrentModelMtx(model);

		this->draw_cube(CUBE_DRAW_ALL_PIECES, 0);
	} else {
		this->draw_cube(CUBE_DRAW_OTHER_PIECES, this->instruction[current].piece);

		guMtxRotAxisDeg(model,
				this->instruction[current].axis,
				revert * this->instruction[current].angle * ((f32)(NUM_ANIMATION_STEPS - step)) / ((f32)NUM_ANIMATION_STEPS));
		guMtxConcat(current_mtx, model, model);
		DR_setCurrentModelMtx(model);

		this->draw_cube(CUBE_DRAW_THIS_PIECE, this->instruction[current].piece);
	}

	DR_setCurrentModelMtx(current_mtx);

	if (this->instruction[this->current_inst].step > 0)
		this->instruction[this->current_inst].step--;
	else
		(this->*(instruction_members[type].apply))(this->current_inst);
}

void Cube::remove_instructions(int pos, int size) {
	for (int i = pos; i < this->num_inst - size; i++)
		this->instruction[i] = this->instruction[i + size];
	this->num_inst -= size;
}

void Cube::insert_instruction(int type, int piece_ref = 0) {
	this->instruction[this->num_inst] = instruction_templates[type];
	this->instruction[this->num_inst].piece_ref = piece_ref;
	this->num_inst++;
}

void Cube::apply_turn_left(int) {
	int index = this->get_edge_index(this->front_face, this->top_edge);
	int adjacent_edge = this->face[this->front_face].edge[(index + 1) % 4];
	int new_face = this->get_adjacent_face(this->front_face, adjacent_edge);
	index = this->get_edge_index(new_face, adjacent_edge);

	this->front_face = new_face;
	this->top_edge = this->face[this->front_face].edge[(index + 1) % 4];

	this->current_inst++;
}

void Cube::apply_turn_right(int) {
	int index = this->get_edge_index(this->front_face, this->top_edge);
	int adjacent_edge = this->face[this->front_face].edge[(index + 3) % 4];
	int new_face = this->get_adjacent_face(this->front_face, adjacent_edge);
	index = this->get_edge_index(new_face, adjacent_edge);

	this->front_face = new_face;
	this->top_edge = this->face[this->front_face].edge[(index + 3) % 4];

	this->current_inst++;
}

void Cube::apply_turn_up(int) {
	int index = this->get_edge_index(this->front_face, this->top_edge);
	int adjacent_edge = this->face[this->front_face].edge[(index + 2) % 4];
	int new_face = this->get_adjacent_face(this->front_face, adjacent_edge);
	index = this->get_edge_index(new_face, adjacent_edge);

	this->front_face = new_face;
	this->top_edge = this->face[this->front_face].edge[index];

	this->current_inst++;
}

void Cube::apply_turn_down(int) {
	int index = this->get_edge_index(this->front_face, this->top_edge);
	int adjacent_edge = this->face[this->front_face].edge[index];
	int new_face = this->get_adjacent_face(this->front_face, adjacent_edge);
	index = this->get_edge_index(new_face, adjacent_edge);

	this->front_face = new_face;
	this->top_edge = this->face[this->front_face].edge[(index + 2) % 4];

	this->current_inst++;
}

void Cube::apply_turn_cw(int) {
	int index = this->get_edge_index(this->front_face, this->top_edge);
	this->top_edge = this->face[this->front_face].edge[(index + 3) % 4];

	this->current_inst++;
}

void Cube::apply_turn_cc(int) {
	int index = this->get_edge_index(this->front_face, this->top_edge);
	this->top_edge = this->face[this->front_face].edge[(index + 1) % 4];

	this->current_inst++;
}

void Cube::apply_rotate_piece_cw_piece(int piece) {
	int colors[12];

	for(int i = 0; i < 4; i++) {
		int edge = this->face[piece].edge[i];
		int adjacent_face = this->get_adjacent_face(piece, edge);
		int edge_index = this->get_edge_index(adjacent_face, edge);
		for (int j = 0; j < 3; j++)
			colors[i * 3 + j] = this->face[adjacent_face].color[(edge_index * 2 + j) % 8];
	}

	for(int i = 0; i < 4; i++) {
		int edge = this->face[piece].edge[i];
		int adjacent_face = this->get_adjacent_face(piece, edge);
		int edge_index = this->get_edge_index(adjacent_face, edge);
		for (int j = 0; j < 3; j++)
			this->face[adjacent_face].color[(edge_index * 2 + j) % 8] = colors[(i * 3 + j + 9) % 12];
	}

	for(int i = 0; i < 8; i++)
		colors[i] = this->face[piece].color[i];
	for(int i = 0; i < 8; i++)
		this->face[piece].color[i] = colors[(i + 6) % 8];
}

void Cube::apply_rotate_piece_cw(int pos) {
	int piece = this->instruction[pos].piece;
	this->apply_rotate_piece_cw_piece(piece);

	this->current_inst++;
	this->num_rotations++;
}

void Cube::apply_rotate_piece_cc_piece(int piece) {
	int colors[12];

	for(int i = 0; i < 4; i++) {
		int edge = this->face[piece].edge[i];
		int adjacent_face = this->get_adjacent_face(piece, edge);
		int edge_index = this->get_edge_index(adjacent_face, edge);
		for (int j = 0; j < 3; j++)
			colors[i * 3 + j] = this->face[adjacent_face].color[(edge_index * 2 + j) % 8];
	}

	for(int i = 0; i < 4; i++) {
		int edge = this->face[piece].edge[i];
		int adjacent_face = this->get_adjacent_face(piece, edge);
		int edge_index = this->get_edge_index(adjacent_face, edge);
		for (int j = 0; j < 3; j++)
			this->face[adjacent_face].color[(edge_index * 2 + j) % 8] = colors[(i * 3 + j + 3) % 12];
	}

	for(int i = 0; i < 8; i++)
		colors[i] = this->face[piece].color[i];
	for(int i = 0; i < 8; i++)
		this->face[piece].color[i] = colors[(i + 2) % 8];
}

void Cube::apply_rotate_piece_cc(int pos) {
	int piece = this->instruction[pos].piece;
	this->apply_rotate_piece_cc_piece(piece);

	this->current_inst++;
	this->num_rotations++;
}

void Cube::apply_undo(int pos) {
	int type = this->instruction[pos - 1].type;
	if ((type == INSTRUCTION_ROTATE_CW) || (type == INSTRUCTION_ROTATE_CC))
		this->num_rotations -= 2;

	(this->*(instruction_members[type].undo))(this->current_inst - 1);
	this->current_inst--;
	this->remove_instructions(this->current_inst - 1, 2);
	this->current_inst--;
}

void Cube::turn_left() {
	this->insert_instruction(INSTRUCTION_TURN_LEFT);
}

void Cube::turn_right() {
	this->insert_instruction(INSTRUCTION_TURN_RIGHT);
}

void Cube::turn_up() {
	this->insert_instruction(INSTRUCTION_TURN_UP);
}

void Cube::turn_down() {
	this->insert_instruction(INSTRUCTION_TURN_DOWN);
}

void Cube::turn_cw() {
	this->insert_instruction(INSTRUCTION_TURN_CW);
}

void Cube::turn_cc() {
	this->insert_instruction(INSTRUCTION_TURN_CC);
}

void Cube::rotate_piece_cw(int piece_ref) {
	this->insert_instruction(INSTRUCTION_ROTATE_CW, piece_ref);
}

void Cube::rotate_piece_cc(int piece_ref) {
	this->insert_instruction(INSTRUCTION_ROTATE_CC, piece_ref);
}

void Cube::undo() {
	int count = 0;

	for (int i = 0; i < this->num_inst; i++) {
		if (this->instruction[i].type != INSTRUCTION_UNDO)
			count++;
		else
			count--;
	}

	if (count > 0)
		this->insert_instruction(INSTRUCTION_UNDO);
}

void Cube::turn(int piece_from, int piece_to) {
	if (piece_from == PIECE_FRONT) {
		switch(piece_to) {
			case PIECE_TOP:    this->turn_up();    return;
			case PIECE_RIGHT:  this->turn_right(); return;
			case PIECE_BOTTOM: this->turn_down();  return;
			case PIECE_LEFT:   this->turn_left();  return;
		}
		return;
	}

	if (piece_from == PIECE_TOP) {
		switch(piece_to) {
			case PIECE_FRONT: this->turn_down(); return;
			case PIECE_LEFT:  this->turn_cc();   return;
			case PIECE_BACK:  this->turn_up();   return;
			case PIECE_RIGHT: this->turn_cw();   return;
		}
		return;
	}

	if (piece_from == PIECE_RIGHT) {
		switch(piece_to) {
			case PIECE_TOP:    this->turn_cc();    return;
			case PIECE_BACK:   this->turn_right(); return;
			case PIECE_BOTTOM: this->turn_cw();    return;
			case PIECE_FRONT:  this->turn_left();  return;
		}
		return;
	}

	if (piece_from == PIECE_BOTTOM) {
		switch(piece_to) {
			case PIECE_FRONT: this->turn_up();   return;
			case PIECE_RIGHT: this->turn_cc();   return;
			case PIECE_BACK:  this->turn_down(); return;
			case PIECE_LEFT:  this->turn_cw();   return;
		}
		return;
	}

	if (piece_from == PIECE_LEFT) {
		switch(piece_to) {
			case PIECE_FRONT:  this->turn_right(); return;
			case PIECE_BOTTOM: this->turn_cc();    return;
			case PIECE_BACK:   this->turn_left();	 return;
			case PIECE_TOP:    this->turn_cw();    return;
		}
		return;
	}

	if (piece_from == PIECE_BACK) {
		switch(piece_to) {
			case PIECE_TOP:    this->turn_down();  return;
			case PIECE_LEFT:   this->turn_right(); return;
			case PIECE_BOTTOM: this->turn_up();    return;
			case PIECE_RIGHT:  this->turn_left();	 return;
		}
		return;
	}
}

void Cube::init_turn(int pos) {
}

void Cube::init_undo(int pos) {
	int type = this->instruction[pos - 1].type;
	if ((type == INSTRUCTION_ROTATE_CW) || (type == INSTRUCTION_ROTATE_CC))
		this->cube_sound[generate() % 4].play(0, 255, 255);
}

void Cube::init_rotate_piece_cw(int pos) {
	Vector *axis = NULL;
	f32 angle = 0.0;
	int piece = 0;
	int edge_index;

	switch(this->instruction[pos].piece_ref) {
		case PIECE_FRONT:
			piece = this->front_face;
			axis = &Z;
			angle = -90.0;
			break;

		case PIECE_TOP:
			piece = this->get_adjacent_face(this->front_face, this->top_edge);
			axis = &Y;
			angle = -90.0;
			break;

		case PIECE_RIGHT:
			edge_index = this->get_edge_index(this->front_face, this->top_edge);
			piece = this->get_adjacent_face(this->front_face, this->face[this->front_face].edge[(edge_index + 1) % 4]);
			axis = &X;
			angle = -90.0;
			break;

		case PIECE_BOTTOM:
			edge_index = this->get_edge_index(this->front_face, this->top_edge);
			piece = this->get_adjacent_face(this->front_face, this->face[this->front_face].edge[(edge_index + 2) % 4]);
			axis = &Y;
			angle = 90.0;
			break;

		case PIECE_LEFT:
			edge_index = this->get_edge_index(this->front_face, this->top_edge);
			piece = this->get_adjacent_face(this->front_face, this->face[this->front_face].edge[(edge_index + 3) % 4]);
			axis = &X;
			angle = 90.0;
			break;

		case PIECE_BACK:
			piece = this->get_adjacent_face(this->front_face, this->top_edge);
			edge_index = this->get_edge_index(piece, this->top_edge);
			piece = this->get_adjacent_face(piece, this->face[piece].edge[(edge_index + 2) % 4]);
			axis = &Z;
			angle = 90.0;
			break;
	}

	this->instruction[pos].axis = axis;
	this->instruction[pos].angle = angle;
	this->instruction[pos].piece = piece;

	this->cube_sound[generate() % 4].play(0, 255, 255);

	if (this->initial_time > gettime())
		this->initial_time = gettime();
}

void Cube::init_rotate_piece_cc(int pos) {
	Vector *axis = NULL;
	f32 angle = 0.0;
	int piece = 0;
	int edge_index;

	switch(this->instruction[pos].piece_ref) {
		case PIECE_FRONT:
			piece = this->front_face;
			axis = &Z;
			angle = 90.0;
			break;

		case PIECE_TOP:
			piece = this->get_adjacent_face(this->front_face, this->top_edge);
			axis = &Y;
			angle = 90.0;
			break;

		case PIECE_RIGHT:
			edge_index = this->get_edge_index(this->front_face, this->top_edge);
			piece = this->get_adjacent_face(this->front_face, this->face[this->front_face].edge[(edge_index + 1) % 4]);
			axis = &X;
			angle = 90.0;
			break;

		case PIECE_BOTTOM:
			edge_index = this->get_edge_index(this->front_face, this->top_edge);
			piece = this->get_adjacent_face(this->front_face, this->face[this->front_face].edge[(edge_index + 2) % 4]);
			axis = &Y;
			angle = -90.0;
			break;

		case PIECE_LEFT:
			edge_index = this->get_edge_index(this->front_face, this->top_edge);
			piece = this->get_adjacent_face(this->front_face, this->face[this->front_face].edge[(edge_index + 3) % 4]);
			axis = &X;
			angle = -90.0;
			break;

		case PIECE_BACK:
			piece = this->get_adjacent_face(this->front_face, this->top_edge);
			edge_index = this->get_edge_index(piece, this->top_edge);
			piece = this->get_adjacent_face(piece, this->face[piece].edge[(edge_index + 2) % 4]);
			axis = &Z;
			angle = -90.0;
			break;
	}

	this->instruction[pos].axis = axis;
	this->instruction[pos].angle = angle;
	this->instruction[pos].piece = piece;

	this->cube_sound[generate() % 4].play(0, 255, 255);

	if (this->initial_time > gettime())
		this->initial_time = gettime();
}

void Cube::reset() {
	// Movement instructions
	this->num_inst = 0;
	this->current_inst = 0;

	// Set initial colors
	for (int i = 0; i < 6; i++) {
		this->face[i].central_color = i;
		for (int j = 0; j < 8; j++)
			this->face[i].color[j] = i;
	}

	// Set initial position
	this->front_face = 2;
	this->top_edge = 0;

	// If you reset it, it's time is high
	this->state = STATE_SOLVED;
	this->initial_time = gettime();
	this->end_time = this->initial_time + (((long long)TB_TIMER_CLOCK) * ((long long)1000)) * ((long long)600);
}

void Cube::scramble_piece() {
	int movement = generate() % 12;

	if (movement == this->last_scramble_movement) {
		this->scramble_piece();
		return;
	}

	this->last_scramble_movement = movement;

	int piece = movement >> 1;
	if (movement & 1)
		this->apply_rotate_piece_cw_piece(piece);
	else
		this->apply_rotate_piece_cc_piece(piece);
}

void Cube::scramble() {
	this->reset();

	this->last_scramble_movement = -1;

	for (int i = 0; i < 25; i++)
		this->scramble_piece();

	this->num_rotations = 0;
	this->initial_time = gettime() + (TB_TIMER_CLOCK * 1000) * 15;
	this->end_time = gettime();
	this->state = STATE_SOLVING;
}

int Cube::get_num_rotations() {
	return this->num_rotations;
}

f32 Cube::get_current_time() {
	if (this->is_solved() && (this->state == STATE_SOLVING)) {
		this->state = STATE_SOLVED;
		this->end_time = gettime();
	}

	if (this->state == STATE_SOLVING)
		return (((f32)gettime()) - ((f32)this->initial_time)) / ((f32)(TB_TIMER_CLOCK * 1000));
	else
		return (((f32)this->end_time) - ((f32)this->initial_time)) / ((f32)(TB_TIMER_CLOCK * 1000));
}

int Cube::is_solved() {
	for (int i = 0; i < 6; i++) {
		for (int j = 0; j < 8; j++)
			if (this->face[i].color[j] != i)
				return 0;
	}

	return 1;
}
